
/*---------------*/
/* Joseph Porter */
/* Summer of 2012*/
/* Team 1247     */
/*www.ROKSBOT.com*/
/* Drive Train   */
/*---------------*/

package Team1247Classes.DriveTrain;

//Imports\\

import edu.wpi.first.wpilibj.CANJaguar;

import edu.wpi.first.wpilibj.RobotDrive;




public class DriveTrain {
    private final int P = 0;
    private final int I = 0;
    private final int D = 0;
    private CANJaguar.ControlMode controlMode;
    private String dataControlMode;
    private RobotDrive ROBOT_DRIVETRAIN;
    
    
    
    
    //Two Wheel -PWM-\\
    public DriveTrain(int leftMotorChannel,int rightMotorChannel){
         this.ROBOT_DRIVETRAIN = new RobotDrive(leftMotorChannel, rightMotorChannel);
         
         this.dataControlMode = "PWM";
    }
    //Four Wheel -PWM-\\
    public DriveTrain(int leftFrontMotorChannel, int leftRearMotorChannel, int rightFrontMotorChannel, int rightRearMotorChannel){
        this.ROBOT_DRIVETRAIN = new RobotDrive(leftFrontMotorChannel, leftRearMotorChannel, rightFrontMotorChannel, rightRearMotorChannel);
        this.dataControlMode = "PWM";
    }
     
    //Two Wheel -CAN-
    public DriveTrain(int leftMotorChannel,int rightMotorChannel, CANJaguar.ControlMode controlMode){
        CANJaguar leftJaguar;
        CANJaguar rightJaguar;
        
        
        
        
        
        try{
            leftJaguar = new CANJaguar(leftMotorChannel, controlMode);
            rightJaguar = new CANJaguar(rightMotorChannel, controlMode);
            
            if (controlMode == CANJaguar.ControlMode.kCurrent){
                 leftJaguar.setPID(P, I, D);
                 leftJaguar.enableControl();
                 rightJaguar.setPID(P, I, D);
                 rightJaguar.enableControl();
            }
            
                 
            
            this.ROBOT_DRIVETRAIN = new RobotDrive(leftJaguar, rightJaguar);
        }catch (Exception CAN_EXCEPTION){
        System.out.println(CAN_EXCEPTION);
    }
        this.dataControlMode = "PWM";
    }
    
    //Four Wheel -CAN-\\
    public DriveTrain(int leftFrontMotorChannel, int leftRearMotorChannel, int rightFrontMotorChannel, int rightRearMotorChannel, CANJaguar.ControlMode controlMode){
        CANJaguar leftFrontJaguar;
        CANJaguar leftRearJaguar;
        CANJaguar rightFrontJaguar;
        CANJaguar rightRearJaguar;
        
        this.controlMode = controlMode;
        
        
        
        try{
            leftFrontJaguar = new CANJaguar(leftFrontMotorChannel, controlMode);
            leftRearJaguar = new CANJaguar(leftRearMotorChannel, controlMode);
            rightFrontJaguar = new CANJaguar(rightFrontMotorChannel, controlMode);
            rightRearJaguar = new CANJaguar(rightRearMotorChannel, controlMode);
            
            if (controlMode == CANJaguar.ControlMode.kCurrent){
                 leftFrontJaguar.setPID(P, I, D);
                 leftFrontJaguar.enableControl();
                 leftRearJaguar.setPID(P, I, D);
                 leftRearJaguar.enableControl();
                 rightFrontJaguar.setPID(P, I, D);
                 rightRearJaguar.enableControl();
                 rightRearJaguar.setPID(P, I, D);
                 rightRearJaguar.enableControl();
            }
            
            this.ROBOT_DRIVETRAIN = new RobotDrive(leftFrontJaguar, leftRearJaguar, rightFrontJaguar, rightRearJaguar);
            
        }catch (Exception CAN_EXCEPTION){
        System.out.println(CAN_EXCEPTION);
        }        
        this.dataControlMode = "PWM";
    }
    public void TankDrive(double leftAxis, double rightAxis){
        if (this.dataControlMode.equals("PWM")){
            this.ROBOT_DRIVETRAIN.tankDrive(leftAxis, rightAxis);
        }
        if (this.dataControlMode.equals("CAN")){
            try{
                this.ROBOT_DRIVETRAIN.tankDrive(leftAxis, rightAxis);
            }catch(Exception CAN_EXCETION){
                System.out.println(CAN_EXCETION);
            }
        }
    }
    public void ArcadeDrive(double y_Axis, double x_Axis){
        if (this.dataControlMode.equals("PWM")){
            this.ROBOT_DRIVETRAIN.arcadeDrive(y_Axis, x_Axis);
        }
        if (this.dataControlMode.equals("CAN")){
            try{
                this.ROBOT_DRIVETRAIN.arcadeDrive(y_Axis, x_Axis);
            }catch(Exception CAN_EXCETION){
                System.out.println(CAN_EXCETION);
            }
        }
    }
    public void MeccanumDrive(double x_Axis, double y_Axis, double z_Axis){
        if (this.dataControlMode.equals("PWM")){
            this.ROBOT_DRIVETRAIN.mecanumDrive_Cartesian(x_Axis, y_Axis, z_Axis, 0);
        }
        if (this.dataControlMode.equals("CAN")){
            try{
                this.ROBOT_DRIVETRAIN.mecanumDrive_Cartesian(x_Axis, y_Axis, z_Axis, 0);
            }catch(Exception CAN_EXCETION){
                System.out.println(CAN_EXCETION);
            }
        }
    }
    public void MeccanumDrive(double x_Axis, double y_Axis, double z_Axis,double gyroValue){
        if (this.dataControlMode.equals("PWM")){
            this.ROBOT_DRIVETRAIN.mecanumDrive_Cartesian(x_Axis, y_Axis, z_Axis, gyroValue);
        }
        if (this.dataControlMode.equals("CAN")){
            try{
                this.ROBOT_DRIVETRAIN.mecanumDrive_Cartesian(x_Axis, y_Axis, z_Axis, gyroValue);
            }catch(Exception CAN_EXCETION){
                System.out.println(CAN_EXCETION);
            }
        }
    }
    }
    
    

